#include "ulib.h"
#include "sysenv.h"
#include "syscall.h"

#define UIPS_NODE_SIZE 12

struct uips_node_struct_t
{
	 struct
	 {
		  unsigned int wait    : 1;
		  unsigned int ac_wait : 1;
		  unsigned int pid     : 30;
	 } __attribute__ ((packed));

	 uips_node_t *prev, *next;
} __attribute__((aligned (4)));

static char __uips_size_assert[sizeof(uips_node_t) == UIPS_NODE_SIZE ? 0 : -1];
static char __mutex_size_assert[sizeof(mutex_t) == 8 ? 0 : -1];
static char __sem_size_assert[sizeof(semaphore_t) == 12 ? 0 : -1];
static char __bq_size_assert[sizeof(bqueue_t) == 8 ? 0 : -1];

void
uips_wait(uips_node_t *node)
{
	 while (1)
	 {
		  sc_wait_pretend(WLEVEL_USER);
		  if (!node->wait || !node->ac_wait)
			   break;
		  sc_wait_try();
	 }
}

int
uips_wait_try(uips_node_t *node)
{
	 return node->wait && node->ac_wait;
}

int
uips_is_finish(uips_node_t *node)
{
	 return !node->wait;
}

#define NODE_PTR_TO_ID(node) ((uintptr_t)node >> 2)
#define NODE_ID_TO_PTR(node) ((uips_node_t *)((uintptr_t)node << 2))

void
mutex_init(mutex_t *mutex)
{
	 spl_init(&mutex->lock);
	 mutex->hold = 0;
}

int
mutex_try_acquire(mutex_t *mutex)
{
	 int result;
	 spl_acquire(&mutex->lock);
	 if (mutex->hold == 0)
	 {
		  mutex->hold = 1;
		  mutex->wait = 0;
		  
		  result = 0;
	 }
	 else result = 1;
	 spl_release(&mutex->lock);
	 return result;
}

int
mutex_acquire(mutex_t *mutex, uips_node_t *node)
{
	 spl_acquire(&mutex->lock);
	 if (mutex->hold == 0)
	 {
		  mutex->hold = 1;
		  mutex->wait = 0;
		  
		  spl_release(&mutex->lock);

		  return 0;
	 }
	 else
	 {
		  node->pid     = cur_pid();
		  node->ac_wait = 1;
		  node->wait    = 1;
		  
		  if (mutex->wait)
		  {
			   node->next = NODE_ID_TO_PTR(mutex->head);
			   node->prev = node->next->prev;
			   node->next->prev = node;
			   node->prev->next = node;
		  }
		  else
		  {
			   mutex->wait = 1;
			   node->next = node->prev = node;
			   mutex->head = NODE_PTR_TO_ID(node);
		  }

		  spl_release(&mutex->lock);
		  
		  return 1;
	 }
}

void
mutex_ac_break(mutex_t *mutex, uips_node_t *node)
{
	 spl_acquire(&mutex->lock);
	 if (node->wait)
	 {
		  node->next->prev = node->next;
		  node->prev->next = node->prev;
		  
		  if (mutex->head == NODE_PTR_TO_ID(node))
		  {
			   if (node->next == node)
					mutex->wait = 0;
			   else mutex->head = NODE_PTR_TO_ID(node->next);
		  }
	 }
	 spl_release(&mutex->lock);
	 node->ac_wait = 0;
}

void
mutex_release(mutex_t *mutex)
{
	 int notify = PROC_NULL;
	 if (!mutex->hold) return;
	 spl_acquire(&mutex->lock);
	 if (mutex->wait)
	 {
		  uips_node_t *node = NODE_ID_TO_PTR(mutex->head);
		  node->wait = 0;
		  
		  notify = node->pid;
		  node->next->prev = node->prev;
		  node->prev->next = node->next;
		  
		  if (mutex->head == NODE_PTR_TO_ID(node->next))
			   mutex->wait = 0;
		  else mutex->head = NODE_PTR_TO_ID(node->next);
	 }
	 else mutex->hold = 0;
	 spl_release(&mutex->lock);
	 if (notify != PROC_NULL)
		  sc_notify(notify);
}

void
semaphore_init(semaphore_t *semaphore, uint32_t count)
{
	 spl_init(&semaphore->lock);
	 semaphore->count = count;
	 semaphore->wait = 0;
}

int
semaphore_try_acquire(semaphore_t *semaphore)
{
	 int result;
	 spl_acquire(&semaphore->lock);
	 if (semaphore->count > 0)
	 {
		  if (-- semaphore->count == 0)
			   semaphore->wait = 0;
		  result = 0;
	 }
	 else result = 1;
	 spl_release(&semaphore->lock);
	 return result;
}

int
semaphore_acquire(semaphore_t *semaphore, uips_node_t *node)
{
	 spl_acquire(&semaphore->lock);
	 if (semaphore->count > 0)
	 {
		  if (-- semaphore->count == 0)
			   semaphore->wait = 0;
		  
		  spl_release(&semaphore->lock);
		  node->wait = 0;
		  node->ac_wait = 0;
		  return 0;
	 }
	 else
	 {
		  node->pid     = cur_pid();
		  node->ac_wait = 1;
		  node->wait    = 1;

		  if (semaphore->wait)
		  {
			   node->next = NODE_ID_TO_PTR(semaphore->head);
			   node->prev = node->next->prev;
			   node->next->prev = node;
			   node->prev->next = node;
		  }
		  else
		  {
			   semaphore->wait = 1;
			   node->next = node->prev = node;
			   semaphore->head = NODE_PTR_TO_ID(node);
		  }

		  spl_release(&semaphore->lock);
		  return 1;
	 }		  
}

void
semaphore_ac_break(semaphore_t *semaphore, uips_node_t *node)
{
	 spl_acquire(&semaphore->lock);
	 if (node->wait)
	 {
		  node->next->prev = node->next;
		  node->prev->next = node->prev;
		  if (semaphore->head == NODE_PTR_TO_ID(node))
		  {
			   if (node->next == node)
					semaphore->wait = 0;
			   else semaphore->head = NODE_PTR_TO_ID(node->next);
		  }
	 }
	 spl_release(&semaphore->lock);
	 node->ac_wait = 0;
}

int
semaphore_release(semaphore_t *semaphore)
{
	 int result = 0;
	 int notify = PROC_NULL;
	 spl_acquire(&semaphore->lock);
	 if (semaphore->wait)
	 {
		  uips_node_t *node = NODE_ID_TO_PTR(semaphore->head);
		  node->wait = 0;
		  
		  notify = node->pid;
		  node->next->prev = node->prev;
		  node->prev->next = node->next;
		  if (node->next == node)
			   semaphore->wait = 0;
		  else semaphore->head = NODE_PTR_TO_ID(node->next);

		  result = 1;
	 }
	 else
	 {
		  ++ semaphore->count;
	 }
	 spl_release(&semaphore->lock);
	 if (notify != PROC_NULL)
		  sc_notify(notify);

	 return result;
}

#define BQ_NOWAIT  0
#define BQ_WAITPUT 1
#define BQ_WAITGET 2

void
bqueue_init(bqueue_t *bq)
{
	 spl_init(&bq->lock);
	 bq->wait = BQ_NOWAIT;
}

int
bqueue_put(bqueue_t *bq, uips_node_t *node)
{	 
	 spl_acquire(&bq->lock);
	 if (bq->wait == BQ_WAITPUT)
	 {
		  node = NODE_ID_TO_PTR(bq->head);
		  node->wait = 0;
		  
		  node->next->prev = node->prev;
		  node->prev->next = node->next;
		  if (node->next == node)
			   bq->wait = BQ_NOWAIT;
		  else bq->head = NODE_PTR_TO_ID(node->next);

		  spl_release(&bq->lock);
		  sc_notify(node->pid);

		  return 0;
	 }
	 else
	 {
		  node->pid     = cur_pid();
		  node->ac_wait = 1;
		  node->wait    = 1;

		  if (bq->wait == BQ_WAITGET)
		  {
			   node->next = NODE_ID_TO_PTR(bq->head);
			   node->prev = node->next->prev;
			   node->next->prev = node;
			   node->prev->next = node;
		  }
		  else
		  {
			   bq->wait = BQ_WAITGET;
			   node->next = node->prev = node;
			   bq->head = NODE_PTR_TO_ID(node);
		  }

		  spl_release(&bq->lock);
		  return 1;
	 }		  
}

int
bqueue_get(bqueue_t *bq, uips_node_t *node)
{	 
	 spl_acquire(&bq->lock);
	 if (bq->wait == BQ_WAITGET)
	 {
		  node = NODE_ID_TO_PTR(bq->head);
		  node->wait = 0;
		  
		  node->next->prev = node->prev;
		  node->prev->next = node->next;
		  
		  if (node->next == node)
			   bq->wait = BQ_NOWAIT;
		  else bq->head = NODE_PTR_TO_ID(node->next);

		  spl_release(&bq->lock);
		  sc_notify(node->pid);

		  return 0;
	 }
	 else
	 {
		  node->pid     = cur_pid();
		  node->ac_wait = 1;
		  node->wait    = 1;

		  if (bq->wait == BQ_WAITPUT)
		  {
			   node->next = NODE_ID_TO_PTR(bq->head);
			   node->prev = node->next->prev;
			   node->next->prev = node;
			   node->prev->next = node;
		  }
		  else
		  {
			   bq->wait = BQ_WAITPUT;
			   node->next = node->prev = node;
			   bq->head = NODE_PTR_TO_ID(node);
		  }

		  spl_release(&bq->lock);
		  
		  return 1;
	 }		  
}
